import socket
import struct
import time
import requests
from dataclasses import dataclass
from collections import deque
import xml.etree.ElementTree as ET
from PyQt6.QtCore import QObject, pyqtSignal, QThread, QMutex


@dataclass
class CalibrationParams:
    """校准参数数据结构"""
    force_units: str       # 力单位（如"N"）
    torque_units: str      # 扭矩单位（如"Nm"）
    counts_per_force: int  # 每力单位对应的计数
    counts_per_torque: int # 每扭矩单位对应的计数


class NetFTProtocol:
    """NET F/T传感器协议处理类"""
    
    def __init__(self, ip: str = "192.168.1.1"):
        self.sensor_ip = ip
        self.calibration = None
        self.buffer = bytearray()  # 数据缓冲区，用于处理不完整的数据包
        self.packet_size = 36  # RDT数据包大小
    
    def fetch_calibration_params(self):
        """通过XML接口自动获取校准参数"""
        try:
            # 发送HTTP请求获取系统配置
            url = f"http://{self.sensor_ip}/netftapi2.xml?index=0"
            response = requests.get(url, timeout=3)
            response.raise_for_status()
            
            # 解析XML内容
            xml_element_tree = ET.fromstring(response.text)
            
            # 提取校准参数
            cfgcpf = int(self._extract_value(xml_element_tree, "cfgcpf"))
            cfgcpt = int(self._extract_value(xml_element_tree, "cfgcpt"))
            cfgfu = int(self._extract_value(xml_element_tree, "cfgfu"))
            cfgtu = int(self._extract_value(xml_element_tree, "cfgtu"))
            
            # 转换单位代码为名称
            force_units = {
                2: "N",    # 牛顿
                5: "kgf",  # 公斤力
            }.get(cfgfu, "?")
            
            torque_units = {
                3: "Nm",   # 牛米
            }.get(cfgtu, "?")
            
            self.calibration = CalibrationParams(
                force_units=force_units,
                torque_units=torque_units,
                counts_per_force=cfgcpf,
                counts_per_torque=cfgcpt
            )
            
            return True
            
        except Exception as e:
            return False
    
    def _extract_value(self, xml_element_tree, tag_name):
        """使用XML解析器提取指定标签的值"""
        try:
            element = xml_element_tree.find(tag_name)
            if element is not None:
                return element.text.strip()
            else:
                return "1"
        except Exception as e:
            return "1"
    
    def create_start_command(self):
        """创建启动命令"""
        return struct.pack(
            "!HHI", 
            0x1234,   # 命令头
            0x0002,   # 启动高速实时流命令
            0         # 无限样本数
        )
    
    def create_stop_command(self):
        """创建停止命令"""
        return struct.pack("!HHI", 0x1234, 0x0000, 0)
    
    def parse_rdt_packet(self, data):
        """解析RDT数据包"""
        try:
            # 解析数据包（文档9.1节RDT记录结构）
            rdt_seq, ft_seq, status, fx, fy, fz, tx, ty, tz = struct.unpack(
                "!IIIiiiiii", data
            )
            
            # 转换为物理单位
            if self.calibration:
                force_x = fx / self.calibration.counts_per_force
                force_y = fy / self.calibration.counts_per_force
                force_z = fz / self.calibration.counts_per_force
                torque_x = tx / self.calibration.counts_per_torque
                torque_y = ty / self.calibration.counts_per_torque
                torque_z = tz / self.calibration.counts_per_torque
            else:
                force_x = force_y = force_z = 0.0
                torque_x = torque_y = torque_z = 0.0
            
            data = {
                'rdt_sequence': rdt_seq,
                'ft_sequence': ft_seq,
                'status': status,
                'Fx': force_x,
                'Fy': force_y,
                'Fz': force_z,
                'Tx': torque_x,
                'Ty': torque_y,
                'Tz': torque_z,
                'pc_timestamp': time.time_ns() / 1000000
            }

            result = {
                'identifier': '0x61',
                'data': data
            }

            return result
            
        except Exception as e:
            print(f"[NET F/T] 数据包解析错误: {str(e)}")
            return None
        
    def data_received(self, raw_data: bytes):
        """
        处理接收到的原始UDP数据，进行分包和解析
        
        Args:
            raw_data: 从UDP缓冲区接收到的原始数据
        """
        results = []
        
        # 如果数据为空，提前返回
        if not raw_data:
            return results
        
        # 将接收到的数据添加到缓冲区
        self.buffer.extend(raw_data)
        
        # 如果缓冲区小于一个完整数据包大小，直接返回
        if len(self.buffer) < self.packet_size:
            return results
        
        # 查找并解析所有完整的RDT数据包
        delete_position = 0
        for i in range(0, len(self.buffer) - 4):
            if (self.buffer[i] == 0 and 
                self.buffer[i + 1] == 0 and 
                self.buffer[i + 2] == 0 and 
                self.buffer[i + 3] == 0):
                if i > 7 and i + 27 < len(self.buffer):
                    # 提取一个完整的数据包
                    packet = bytes(self.buffer[i-8 : i + 28])
                    results.append(self.parse_rdt_packet(packet))
                    delete_position = i + 28
        # 删除已处理的数据包
        self.buffer = self.buffer[delete_position:]

        return results
    


class UDPThread(QThread):
    """UDP数据接收线程"""
    
    data_received = pyqtSignal(list)  # 数据接收信号
    message_signal = pyqtSignal(str)  # 用于发送消息到UI
    
    def __init__(self, sensor_ip: str , udp_port: int, protocol: NetFTProtocol):
        super().__init__()
        self.sensor_ip = sensor_ip
        self.udp_port = udp_port
        self.protocol = protocol
        self.socket = None
        self.is_running = False
        self.protocol_mutex = QMutex()
        self.last_sequence = -1
        self.drop_count = 0
    
    def start_streaming(self):
        """启动数据流"""
        if self.is_running:
            return False

        try:
            # 创建UDP socket
            self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            self.socket.bind(("0.0.0.0", self.udp_port))
            self.socket.setblocking(False)  # 设置为非阻塞模式
            
            # 发送启动命令
            self.protocol_mutex.lock()
            command = self.protocol.create_start_command()
            self.protocol_mutex.unlock()
            self.socket.sendto(command, (self.sensor_ip, self.udp_port))
            
            # 启动接收线程
            self.is_running = True
            self.start()
            return True
            
        except Exception as e:
            return False
    
    def stop_streaming(self):
        """停止数据流"""
        if not self.is_running:
            return
            
        self.is_running = False
        
        try:
            # 发送停止命令
            if self.socket:
                self.protocol_mutex.lock()
                command = self.protocol.create_stop_command()
                self.protocol_mutex.unlock()
                self.socket.sendto(command, (self.sensor_ip, self.udp_port))
        except:
            pass
        
        # 等待线程结束
        self.wait(1000)  # 等待1秒
        
        if self.socket:
            self.socket.close()
            self.socket = None

    
    def run(self):
        """线程运行方法"""
        while self.is_running:
            try:
                # 非阻塞接收，读取硬件缓冲区中的所有数据
                data, addr = self.socket.recvfrom(4096)  # 增大接收缓冲区
            
                # 如果有数据，交给协议层处理
                self.protocol_mutex.lock()
                parsed_packets = self.protocol.data_received(data)
                self.protocol_mutex.unlock()
                
                # 处理每个解析成功的数据包
                for parsed_data in parsed_packets:
                    # 检测丢包
                    rdt_seq = parsed_data['data']['rdt_sequence']
                    if self.last_sequence != -1 and (rdt_seq != self.last_sequence + 1):
                        if not (rdt_seq == 0 and self.last_sequence == 0xFFFFFFFF):
                            self.drop_count += 1
                            self.message_signal.emit(f"[NET F/T] 丢包检测：期望{self.last_sequence+1}，实际{rdt_seq}，累计{self.drop_count}")
                    self.last_sequence = rdt_seq
                
                # 发射数据信号
                if parsed_packets:
                    self.data_received.emit(parsed_packets)
                
            except socket.error as e:
                # Windows下的非阻塞套接字错误码
                if e.errno == 10035:  # WSAEWOULDBLOCK - 没有数据可读
                    # 这是正常情况，稍作休眠避免CPU占用过高
                    self.msleep(1)  # 1ms
                    continue
                else:
                    # 其他socket错误
                    if self.is_running:
                        self.message_signal.emit(f"Socket错误: {str(e)}")
                    break
            except Exception as e:
                # 其他异常
                if self.is_running:
                    self.message_signal.emit(f"UDP接收错误: {str(e)}")
                break


class NetFTService(QObject):
    """NET F/T传感器服务类"""
    message_signal = pyqtSignal(str)  # 用于发送消息到UI
       
    def __init__(self, sensor_ip: str = "192.168.1.1", udp_port: int = 49152, callaback=None):
        super().__init__()
        self.sensor_ip = sensor_ip
        self.udp_port = udp_port
        self.data_received_callback = callaback
        
        # 初始化协议和UDP线程
        self.protocol = NetFTProtocol(sensor_ip)
        self.udp_thread = None
        self.is_streaming = False
        
        self.identifier = "0x61"  # 数据标识符
    
    def start_streaming(self):
        """启动数据流"""
        if self.is_streaming:
            self.message_signal.emit("数据流已在运行中")
            return False
        
        # 首先获取校准参数
        if not self.protocol.fetch_calibration_params():
            self.message_signal.emit("校准参数获取失败，无法启动数据流")
            return False
        else:
            params = self.get_calibration_info()
            self.message_signal.emit(f"校准参数获取成功,单位为 {params['force_units']} 和 {params['torque_units']}")
        
        # 创建并启动UDP线程
        self.udp_thread = UDPThread(self.sensor_ip, self.udp_port, self.protocol)
        self.udp_thread.data_received.connect(self._on_data_received)
        self.udp_thread.message_signal.connect(self.message_signal.emit)
        
        if self.udp_thread.start_streaming():
            self.is_streaming = True
            self.message_signal.emit("[NET F/T]数据流已启动")
            return True
        else:
            self.message_signal.emit("[NET F/T]启动数据流失败")
            return False
    
    def stop_streaming(self):
        """停止数据流"""
        if not self.is_streaming:
            self.message_signal.emit("[NET F/T]数据流未在运行中")
            return
        
        if self.udp_thread:
            self.udp_thread.stop_streaming()
            self.udp_thread = None
        
        self.is_streaming = False
        self.message_signal.emit("[NET F/T]数据流已停止")
    
    def _on_data_received(self, data):
        """处理接收到的数据"""
        if self.data_received_callback:
            self.data_received_callback(data)
        
    def get_calibration_info(self):
        """获取校准信息"""
        if self.protocol.calibration:
            return {
                'force_units': self.protocol.calibration.force_units,
                'torque_units': self.protocol.calibration.torque_units,
                'counts_per_force': self.protocol.calibration.counts_per_force,
                'counts_per_torque': self.protocol.calibration.counts_per_torque
            }
        return None
    
    def is_running(self):
        """检查是否正在运行"""
        return self.is_streaming